/**
 * @file gray_filter.cpp
 * @author fwy (2643660853@qq.com)
 * @brief
 * @version 0.1
 * @date 2025-03-25
 *
 * @copyright Copyright (c) 2025
 *
 */

#include <cmath>
#include <utility>
#include <memory>
#include <string>

#include "nav2_util/occ_grid_values.hpp"
#include "fishbot_costmap_filter/gray_filter.hpp"
#include "nav2_costmap_2d/costmap_filters/filter_values.hpp"

namespace nav2_costmap_2d
{

    GrayFilter::GrayFilter()
        : filter_info_sub_(nullptr), mask_sub_(nullptr),
          gray_state_pub_(nullptr), filter_mask_(nullptr), mask_frame_(""), global_frame_(""),
          default_state_(0), gray_state_(default_state_)
    {
    }
    /**
     * @brief 初始化函数
     *
     * @param filter_info_topic mask的订阅
     *        type代表过滤器类型，0代表禁区过滤器，1和2代表速度过滤器
     *        base 数据转换系数中的基础值，用于将OccupancyGrid值转换为过滤器空间中的值，转换公式为：filter_value = OccupancyGrid_value * multiplier + base
     *        filter_mask_topic代表订阅的地图的话题
     */
    void GrayFilter::initializeFilter(
        const std::string &filter_info_topic)
    {
        std::lock_guard<CostmapFilter::mutex_t> guard(*getMutex());

        rclcpp_lifecycle::LifecycleNode::SharedPtr node = node_.lock();
        if (!node)
        {
            throw std::runtime_error{"Failed to lock node"};
        }

        // Declare parameters specific to  only 声明GrayFilter参数
        // 其中default-state代表是否变化，变化状态有0,1,2
        // gray_state代表需要发布的话题名称，该话题数据表示为std_msgs::msg::Int16类型，表示机器人所在地图的灰度区域所代表的状态
        // flip_threshold1代表灰度阈值1，flip_threshold2代表灰度阈值2
        // 灰度为0-flip_threshold1时状态为0，flip_threshold1-flip_threshold2时状态为1，flip_threshold2-255时状态为2
        std::string gray_state_topic;
        declareParameter("default_state", rclcpp::ParameterValue(4));
        node->get_parameter(name_ + "." + "default_state", default_state_);
        declareParameter("gray_state_topic", rclcpp::ParameterValue("gray_state"));
        node->get_parameter(name_ + "." + "gray_state_topic", gray_state_topic);
        declareParameter("flip_threshold1", rclcpp::ParameterValue(30.0));
        node->get_parameter(name_ + "." + "flip_threshold1", flip_threshold1_);
        declareParameter("flip_threshold2", rclcpp::ParameterValue(60.0));
        node->get_parameter(name_ + "." + "flip_threshold2", flip_threshold2_);

        filter_info_topic_ = filter_info_topic;
        // 订阅filter_info_topic
        RCLCPP_INFO(
            logger_,
            "GrayFilter: Subscribing to \"%s\" topic for filter info...",
            filter_info_topic_.c_str());
        filter_info_sub_ = node->create_subscription<nav2_msgs::msg::CostmapFilterInfo>(
            filter_info_topic_, rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable(),
            std::bind(&GrayFilter::filterInfoCallback, this, std::placeholders::_1));

        // 获取代价地图的frame，用于tf变换,@todo layered_costmap_何时更新
        global_frame_ = layered_costmap_->getGlobalFrameID();

        // 发布灰度状态，0，1，2
        gray_state_pub_ = node->create_publisher<std_msgs::msg::Int16>(
            gray_state_topic, rclcpp::QoS(10));
        gray_state_pub_->on_activate();

        // 读取nav2源码中filter_values.hpp的参数
        // BASE_DEFAULT为0
        // MULTIPLIER_DEFAULT为1
        base_ = BASE_DEFAULT;
        multiplier_ = MULTIPLIER_DEFAULT;

        // 将灰度状态初始化为4
        changeState(default_state_);
    }

    /**
     * @brief filter_info_topic的回调函数
     *
     * @param msg
     *      type代表过滤器类型，0代表禁区过滤器，1和2代表速度过滤器，3表示二值过滤器，现在定义4为灰度过滤器
     *      base 数据转换系数中的基础值，用于将OccupancyGrid值转换为过滤器空间中的值，转换公式为：filter_value = OccupancyGrid_value * multiplier + base
     *      filter_mask_topic代表订阅的地图的话题
     */
    void GrayFilter::filterInfoCallback(
        const nav2_msgs::msg::CostmapFilterInfo::SharedPtr msg)
    {
        std::lock_guard<CostmapFilter::mutex_t> guard(*getMutex());

        rclcpp_lifecycle::LifecycleNode::SharedPtr node = node_.lock();
        if (!node)
        {
            throw std::runtime_error{"Failed to lock node"};
        }

        if (!mask_sub_)
        {
            RCLCPP_INFO(
                logger_,
                "GrayFilter: Received filter info from %s topic.", filter_info_topic_.c_str());
        }
        else
        {
            RCLCPP_WARN(
                logger_,
                "GrayFilter: New costmap filter info arrived from %s topic. Updating old filter info.",
                filter_info_topic_.c_str());
            // Resetting previous subscriber each time when new costmap filter information arrives
            mask_sub_.reset();
        }

        if (msg->type != 4)
        {
            RCLCPP_ERROR(logger_, "GrayFilter: Mode %i is not supported", msg->type);
            return;
        }

        // 设置数据转换系数base和multiplier
        base_ = msg->base;
        multiplier_ = msg->multiplier;
        // mask_topic_为nav_msgs::msg::OccupancyGrid数据类型的话题，是蒙版地图
        mask_topic_ = msg->filter_mask_topic;

        // 更新mask_sub_
        RCLCPP_INFO(
            logger_,
            "GrayFilter: Subscribing to \"%s\" topic for filter mask...",
            mask_topic_.c_str());
        mask_sub_ = node->create_subscription<nav_msgs::msg::OccupancyGrid>(
            mask_topic_, rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable(),
            std::bind(&GrayFilter::maskCallback, this, std::placeholders::_1));
    }
    /**
     * @brief filter_info_topic的filter_mask_topic话题回调函数
     *
     * @param msg 图的二维网格数据
     *        header 消息的头部信息，包括时间戳，参考系
     *        info 包含地图的基本信息，分辨率，宽度，高度，以及原点
     *        data 是一个一维数组，表示地图中每个单元格的占用情况。数组中的每个值代表对应单元格的占用概率，取值范围通常为0到100
     */
    void GrayFilter::maskCallback(
        const nav_msgs::msg::OccupancyGrid::SharedPtr msg)
    {
        std::lock_guard<CostmapFilter::mutex_t> guard(*getMutex());

        if (!filter_mask_)
        {
            RCLCPP_INFO(
                logger_,
                "GrayFilter: Received filter mask from %s topic.", mask_topic_.c_str());
        }
        else
        {
            RCLCPP_WARN(
                logger_,
                "GrayFilter: New filter mask arrived from %s topic. Updating old filter mask.",
                mask_topic_.c_str());
            filter_mask_.reset();
        }
        // 将filter_mask_更新为蒙版地图的信息
        filter_mask_ = msg;
        mask_frame_ = msg->header.frame_id;
    }

    /**
     * @brief @todo
     * @param pose global坐标系下机器人的位姿，一般为map坐标系
     */
    void GrayFilter::process(
        nav2_costmap_2d::Costmap2D & /*master_grid*/,
        int /*min_i*/, int /*min_j*/, int /*max_i*/, int /*max_j*/,
        const geometry_msgs::msg::Pose2D &pose)
    {
        std::lock_guard<CostmapFilter::mutex_t> guard(*getMutex());
        // 确认收到地图
        if (!filter_mask_)
        {
            // Show warning message every 2 seconds to not litter an output
            RCLCPP_WARN_THROTTLE(
                logger_, *(clock_), 2000,
                "GrayFilter: Filter mask was not received");
            return;
        }

        geometry_msgs::msg::Pose2D mask_pose; // robot coordinates in mask frame
        // tf变换，获取mask坐标下的机器人位姿mask_pose
        if (!transformPose(global_frame_, pose, mask_frame_, mask_pose))
        {
            return;
        }

        // Converting mask_pose robot position to filter_mask_ indexes (mask_robot_i, mask_robot_j)
        // ?
        unsigned int mask_robot_i, mask_robot_j;
        if (!worldToMask(filter_mask_, mask_pose.x, mask_pose.y, mask_robot_i, mask_robot_j))
        {
            // Robot went out of mask range. Set "false" state by-default
            RCLCPP_WARN(
                logger_,
                "GrayFilter: Robot is outside of filter mask. Resetting gray state to default.");
            changeState(default_state_);
            return;
        }

        // Getting filter_mask data from cell where the robot placed
        int8_t mask_data = getMaskData(filter_mask_, mask_robot_i, mask_robot_j);
        if (mask_data == nav2_util::OCC_GRID_UNKNOWN)
        {
            // Corresponding filter mask cell is unknown.
            // Warn and do nothing.
            RCLCPP_WARN_THROTTLE(
                logger_, *(clock_), 2000,
                "GrayFilter: Filter mask [%i, %i] data is unknown. Do nothing.",
                mask_robot_i, mask_robot_j);
            return;
        }
        std::cout<<"gray_filter:::base_ + mask_data * multiplier_ = "<<base_ + mask_data * multiplier_<<std::endl;

        // Check and flip binary state, if necessary
        if ((base_ + mask_data * multiplier_ <= flip_threshold1_))
        {
            changeState(0);
        }
        else if ((base_ + mask_data * multiplier_ > flip_threshold1_) && (base_ + mask_data * multiplier_ <= flip_threshold2_))
        {
            changeState(1);
        }
        else
        {
            changeState(2);
        }
    }

    void GrayFilter::resetFilter()
    {
        std::lock_guard<CostmapFilter::mutex_t> guard(*getMutex());

        RCLCPP_INFO(logger_, "GrayFilter: Resetting the filter to default state");
        changeState(default_state_);

        filter_info_sub_.reset();
        mask_sub_.reset();
        if (gray_state_pub_)
        {
            gray_state_pub_->on_deactivate();
            gray_state_pub_.reset();
        }
    }

    bool GrayFilter::isActive()
    {
        std::lock_guard<CostmapFilter::mutex_t> guard(*getMutex());

        if (filter_mask_)
        {
            return true;
        }
        return false;
    }

    void GrayFilter::changeState(const int state)
    {
        gray_state_ = state;
        if (prev_gray_state_ != gray_state_)
        {
            RCLCPP_INFO(logger_, "GrayFilter: Switched on");
            // Forming and publishing new GrayState message
            std::unique_ptr<std_msgs::msg::Int16> msg =
                std::make_unique<std_msgs::msg::Int16>();
            msg->data = state;
            RCLCPP_INFO(logger_, "GrayFilter: publish %d to gray_state_topic",state);
            gray_state_pub_->publish(std::move(msg));
        }
        else
        {
            RCLCPP_INFO(logger_, "GrayFilter: Switched off");
        }
        prev_gray_state_ = state;

    }

} // namespace nav2_costmap_2d

#include "pluginlib/class_list_macros.hpp"
PLUGINLIB_EXPORT_CLASS(nav2_costmap_2d::GrayFilter, nav2_costmap_2d::Layer)